/*
 * pps_pll.c
 *
 *  Created on: 17 juin 2014
 *      Author: marioc
 */


#include <stdlib.h>
#include <inttypes.h>
#include "hcpp/ptp2_log.h"
#include "hcpp/pps_pll.h"

//#include "fsm/ptp2_fsm.h"

#include "hal/pps_pll_hal.h"

#define PHASE_STABILITY_MARGIN  (25)            // 250ns hysteresis
#define PHASE_STABILITY_LIM     (100)           // 100ns stability limit
#define PHASE_BUILDOUT_LIM      (15000000)      // 15ms default value to align PPSs
#define PPS_DEFAULT_WIDTH       (200000000)     // 200 ms duration

bool
pps_pll_init(pps_pll_t *pps_pll, ptp2_master_t *master)
{
    float                   ki;
    float                   kp;

    pps_pll->stable                 = false;     /**< dummy value. TODO: implement validity */
    pps_pll->buildout_num           = 0;
    pps_pll->limit_counter          = 0;
    pps_pll->ovf_counter			= 0;
    pps_pll->pps_width              = PPS_DEFAULT_WIDTH;
    pps_pll_hal_init(pps_pll->pps_width);

    pps_pll_limits_get(&pps_pll->lim_tmax, &pps_pll->lim_tmin);

    PTP2_LOG_PRINTLN(PTP2_LOG_PPS_PLL, PTP2_LOG_INFO, ("pps_width:%"PRIu32", lim_min:%"PRIu32", lim_max:%"PRIu32" ", pps_pll->pps_width, pps_pll->lim_tmin, pps_pll->lim_tmax));

    // TODO set filter initial values
    pps_pll_filter_get(&ki, &kp, 0);
    PTP2_LOG_PRINTLN(PTP2_LOG_PPS_PLL, PTP2_LOG_INFO, ("Filter A ki:%f, kp:%f", ki, kp));

    pps_pll_filter_get(&ki, &kp, 1);
    PTP2_LOG_PRINTLN(PTP2_LOG_PPS_PLL, PTP2_LOG_INFO, ("Filter B ki:%f, kp:%f", ki, kp));


    pps_pll_phase_get(&pps_pll->delta_1pps);
    pps_pll->last_delta_1pps    = pps_pll->delta_1pps;
    pps_pll->avg_delta_1pps     = pps_pll->delta_1pps;

    return true;
}




// TODO improve filtering once the behavior of the PLL is known.
uint32_t
pps_pll_avg(int32_t previous, int32_t current)
{
    uint32_t result = (current - previous)*0.8;

    return result;
}


bool
pps_pll_is_stable(int32_t current, int32_t last)
{
    bool result = true;

    if (abs(last - current) < PHASE_STABILITY_LIM) {
        if (result == true) {
            if (current < PHASE_STABILITY_LIM - PHASE_STABILITY_MARGIN) {
                result = false;
            }

        } else {
            if (current  > PHASE_STABILITY_LIM + PHASE_STABILITY_MARGIN) {
                result = true;
            }
        }
    }

    return result;
}

/**
 * Brief:   monitor the PPS PLL
 *          Perform data acquisition from NIOS internal registers;
 *          Check limits and increment counters if needed;
 *          Check if build-out occurs.
 *
 * @param   PPS PLL data structure
 * @return  none
 *
 */
void
pps_pll_monitor(pps_pll_t *pps_pll)
{
    if (NULL != pps_pll) {
        host_pll_pps_status_t reg;

        pps_pll_status_reg_get  (&reg);
        if (reg.reg_t.updated) {
            if (reg.reg_t.limit_reached) {
                pps_pll->limit_counter++;
                PTP2_LOG_PRINTLN(PTP2_LOG_PPS_PLL, PTP2_LOG_INFO, ("PPS_PLL Status %8"PRIu32"", reg.raw));
                PTP2_LOG_PRINTLN(PTP2_LOG_PPS_PLL, PTP2_LOG_INFO, ("PPS_PLL Limit counter %"PRIu32"", pps_pll->limit_counter));
            }


            pps_pll_phase_get(&pps_pll->delta_1pps);
            pps_pll_period_get(&pps_pll->dpll_period);
            //pps_pll_accumulator_get(&pps_pll->accumulator);

            PTP2_LOG_PRINTLN(PTP2_LOG_PPS_PLL, PTP2_LOG_VERBOSE, ("Delta_pps:%"PRIi32", period:%"PRIu32", lim_cnt:%"PRIu32", ovf_cnt:%"PRIu32"",
            		pps_pll->delta_1pps, pps_pll->dpll_period, pps_pll->limit_counter, pps_pll->ovf_counter));


            pps_pll->avg_delta_1pps = pps_pll_avg(pps_pll->avg_delta_1pps, pps_pll->delta_1pps);
            if (pps_pll->avg_delta_1pps > PHASE_BUILDOUT_LIM) {
                pps_pll->last_phase_buildout = abs(PHASE_BUILDOUT_LIM - pps_pll->avg_delta_1pps);
                pps_pll->buildout_num++;
                pps_pll_resync();
                PTP2_LOG_PRINTLN(PTP2_LOG_PPS_PLL, PTP2_LOG_INFO, ("PPS_PLL Resync. Phase buildout:%"PRIi32", Buildout num:%"PRIu32"", pps_pll->last_phase_buildout, pps_pll->buildout_num));

            } else if (1 == reg.reg_t.overflow) {
            	pps_pll->ovf_counter++;
            	PTP2_LOG_PRINTLN(PTP2_LOG_PPS_PLL, PTP2_LOG_INFO, ("PPS_PLL OVF. OVF Count:%"PRIu32"", pps_pll->ovf_counter));
            }
            else {
                pps_pll->stable = pps_pll_is_stable(pps_pll->avg_delta_1pps, pps_pll->last_delta_1pps);
            }
            pps_pll->last_delta_1pps = pps_pll->delta_1pps;
        }



    }
}



